/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package org.usfirst.team3132.demo;

import org.usfirst.team3132.demo.libs.MotorDrive;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Timer;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot extends SimpleRobot
{
    MotorDrive leftMotorDrive;
    MotorDrive rightMotorDrive;
    
    public Robot()
    {
        System.out.println("It's alive");
        
        //Disable watchdog.
        //drive.setSafetyEnabled(false);
        
        leftMotorDrive = new MotorDrive(1,1,3,4,2,2,2);
        rightMotorDrive = new MotorDrive(2,2,1,2,2,2,2);
    }
    
    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() 
    {
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() 
    {
        System.out.println("operator mode");    
        leftMotorDrive.init();
        //rightMotorDrive.init();
        
        while (isOperatorControl() && isEnabled()) // loop during enabled teleop mode
        {   
            leftMotorDrive.loop();
            //rightMotorDrive.loop();
            Timer.delay(0.005);
        }
        
        leftMotorDrive.die();
        //rightMotorDrive.die();
    }
}